#include "send.h"

unsigned char SPIInit() {
    system("gpio load spi");
    long f = 1000000;
    // cout << "Freq " << f << endl;
    if (wiringPiSPISetup(0, f) < 0) {
        //printf("Error:  SPI Setup failed\n");
        return SPI_SETUP_ERROR;
    }    
    //printf("SPI Setup succeeded\n");
    return SPI_SETUP_SUCCESS;
}

unsigned char send(unsigned char inst) {
    unsigned char buffer[] = { inst };
    //buffer[1] = 0;
    //printf("Writing data:  %x\n", buffer[0]);
    wiringPiSPIDataRW(0, buffer, 1);

    struct timespec tim, tim2;
    tim.tv_sec = 0;
    tim.tv_nsec = 1000000;

    if(nanosleep(&tim , &tim2) < 0 )   
    {
       //printf("Nano sleep system call failed \n");
       return -1;
    }

    //printf("Data written.  Data received:  %x\n", buffer[0]);
    return buffer[0];
}

bool resetSPI(int numAttempts) {
    unsigned char data = send(SEND_MOTOR_STOP),
                  oldData = SEND_MOTOR_STOP;
    // send should only ever return 0-3 for non-motor commands
    // Also, it should return the SAME value
    while (data != oldData && data > 3 && (--numAttempts > 0 || numAttempts < 0)) {
        oldData = data; // will happen at least once; we get at least two sends and ensure equivalence
        data = send(SEND_MOTOR_STOP);
    }

    return data == oldData && data < 4;
}

/*
int main() {
    unsigned char data;
    if (SPIInit() == SPI_SETUP_SUCCESS) {
        printf("SPI Setup succeeded\n");
        while (true) {
            printf("Please enter SPI data:  ");
            scanf("%x", &data);
            send(data);
        }
    }
}
*/
